package kit.edu.lego.gruppe3.levels.treeArea;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import kit.edu.lego.gruppe3.levels.treeArea.entrance.EntranceSolver;
import lejos.nxt.LCD;
import lejos.nxt.TouchSensor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;

public class RotateMovement implements Behavior{
	
	public static final float HALF_AREA_DISTANCE = 2000;
	private static float distanceMessured = 0;
	private static float distanceRightMessured = 0;
	public static volatile boolean triggerTouch = true;
	
	private int rotateCount = 0;
	
	private DifferentialPilot robot = ComponentsRepository.getInstance().getRobotPilot();
	private TouchSensor leftTouch = ComponentsRepository.getInstance().getLeftTouchSensor();
	private TouchSensor rigthTouch = ComponentsRepository.getInstance().getRightTouchSensor();
	
	private int rotateAngle = -90; // right rotate
	
	
	@Override
	public void action() {
		LCD.drawString("rotate "+(distanceMessured > HALF_AREA_DISTANCE),0,4);
		robot.rotate(rotateAngle);
		
		float travel = 0.5f;
		while (travel <= 3f){
			robot.travel(travel);
			if (leftTouch.isPressed() || rigthTouch.isPressed()){
				// found object - double lock patter
				if (!TreeAreaSolver.objectFound){
					TreeAreaSolver.objectFound = true;
					
					EntranceSolver entranceSolver = new EntranceSolver();
					entranceSolver.start();
					break;
				}
				
			}
			travel += 1.1f;
		}
		
		if (!TreeAreaSolver.objectFound){
			robot.rotate(rotateAngle);

			distanceMessured = 0;
			distanceRightMessured = 0;
			ComponentsRepository.getInstance().getLeftWheel().resetTachoCount();
			ComponentsRepository.getInstance().getRightWheel().resetTachoCount();
			rotateAngle = (-1) * rotateAngle;
		}
		
		rotateCount++;
		
		if (rotateCount % 2 == 0){
			triggerTouch = false;
			robot.setTravelSpeed(5);
			robot.rotate(-180);
			while (!ComponentsRepository.getInstance().getRightTouchSensor().isPressed() ||
					!ComponentsRepository.getInstance().getLeftTouchSensor().isPressed()){
				robot.forward();
			}
			robot.backward();
			robot.forward();
			robot.travel(-10f);
			robot.rotate(180);
			triggerTouch = true;
		}
		
	}

	@Override
	public void suppress() {
		// TODO Auto-generated method stub
		
	}

	@Override
	public boolean takeControl() {
		distanceMessured = ComponentsRepository.getInstance().getLeftWheel().getTachoCount();
		distanceRightMessured = ComponentsRepository.getInstance().getRightWheel().getTachoCount();
		LCD.clear();
		LCD.drawString("Traveled "+distanceMessured, 0,1);
		LCD.drawString("Traveled "+distanceRightMessured, 0,5);
		return LevelSelector.getInstance().isTreeArea() &&
			(distanceMessured > HALF_AREA_DISTANCE) &&
			!TreeAreaSolver.objectFound;
	}

}
